#include "pid.h"
#include "stm32f407xx.h"

pid_param speed={30,2,1,3000,0,0,0,0,0};
pid_param angle={0.5,0.005,0,500,0,0,0,0,0};

pid_param M3508_speed_1={10,0.5,0,3000,0,0,0,0,0};
pid_param M3508_speed_2={10,0.5,0,3000,0,0,0,0,0};
pid_param M3508_speed_3={10,0.5,0,3000,0,0,0,0,0};
pid_param M3508_speed_4={10,0.5,0,3000,0,0,0,0,0};

/**
 * @brief PID控制函数，输入pid参数，测量值和目标值，输出需要的输出值
 * 
 * @param pid_para PID参数结构体变量
 * @param measure 测量值
 */
void PID_control(pid_param *pid_param,int16_t measure,int16_t target){
    pid_param->target=target;
    pid_param->measure=measure;
    pid_param->error=pid_param->target-pid_param->measure;
    pid_param->error_sum+=pid_param->error;
    if(pid_param->error_sum > pid_param->limit) pid_param->error_sum=pid_param->limit;//i限幅

    pid_param->p=pid_param->kp*pid_param->error;
    pid_param->i=pid_param->ki*pid_param->error_sum;
    pid_param->d=pid_param->kd*(pid_param->error-pid_param->last_error);
    
    pid_param->out=pid_param->p+pid_param->i+pid_param->d;
    pid_param->last_error=pid_param->error;

    if(pid_param->out >= 16000) pid_param->out =16000;
    if(pid_param->out <= -16000) pid_param->out =-16000;
}

/**
 * @brief 用于角度环PID的计算函数
 * 
 * @param pid_param out输出值传输给速度环的target
 * @param measure 接收角度测量值
 * @param target 目标角度
 */
void PID_angle_control(pid_param *pid_param,int16_t measure,int16_t target){
    // 纠正半圈路径
    if(measure-target>4096) target+=8191;
    else if(measure-target<-4096) measure+=8191;

    pid_param->target=target;
    pid_param->measure=measure;
    pid_param->error=pid_param->target-pid_param->measure;
    pid_param->error_sum+=pid_param->error;
    if(pid_param->error_sum > pid_param->limit) pid_param->error_sum=pid_param->limit;//i限幅

    pid_param->p=pid_param->kp*pid_param->error;
    pid_param->i=pid_param->ki*pid_param->error_sum;
    pid_param->d=pid_param->kd*(pid_param->error-pid_param->last_error);
    
    pid_param->out=pid_param->p+pid_param->i+pid_param->d;
    pid_param->last_error=pid_param->error;

    if(pid_param->out >= 16000) pid_param->out =16000;
    if(pid_param->out <= -16000) pid_param->out =-16000;
}

int Limit_Min_Max(int value,int min,int max);

/**
 * @brief PID数组初始化
 * @param PID PID数组
 * @param kp 
 * @param ki 
 * @param kd 
 * @param i_max 
 * @param out_max 
 */
void PID_init(PID_struct_t *PID,
              float kp,
              float ki,
              float kd,
              float i_max,
              float out_max)//PID初始化函数
{
  PID->kp      = kp;
  PID->ki      = ki;
  PID->kd      = kd;
  PID->i_max   = i_max;//积分限幅
  PID->out_max = out_max;//输出限幅
}

 void PID_Protect_Angle(PID_struct_t *pid)
{
	if(pid->ref - pid->fdb > 4096)
	{
		pid->fdb+=8192;
	}
	else if(pid->ref - pid->fdb < -4096)
	{
		pid->fdb-=8192;
	}
}

void PID_Protect_Ink(PID_struct_t *pid)
{
	if(pid->ref - pid->fdb > 180)
	{
		pid->fdb+=360;
	}
	else if(pid->ref - pid->fdb < -180)
	{
		pid->fdb-=360;
	}
}

float PID_Calc_Angle(PID_struct_t *PID, float ref, float fdb)//PID运算函数（目标，实际）
{
  PID->ref = ref;
  PID->fdb = fdb;

	PID_Protect_Angle(PID);//过零保护

  PID->err[1] = PID->err[0];
  PID->err[0] = PID->ref - PID->fdb;
  
  PID->p_out  = PID->kp * PID->err[0];
  PID->i_out += PID->ki * PID->err[0];
  PID->d_out  = PID->kd * (PID->err[0] - PID->err[1]);
  PID->i_out=Limit_Min_Max(PID->i_out, -PID->i_max, PID->i_max);
  
  PID->output = PID->p_out + PID->i_out + PID->d_out;
  PID->output=Limit_Min_Max(PID->output, -PID->out_max, PID->out_max);
  return PID->output;
}

float PID_Calc_Ink(PID_struct_t *PID, float ref, float fdb)//PID运算函数（目标，实际）
{
  PID->ref = ref;
  PID->fdb = fdb;

	PID_Protect_Ink(PID);//过零保护

  PID->err[1] = PID->err[0];
  PID->err[0] = PID->ref - PID->fdb;
  
  PID->p_out  = PID->kp * PID->err[0];
  PID->i_out += PID->ki * PID->err[0];
  PID->d_out  = PID->kd * (PID->err[0] - PID->err[1]);
  PID->i_out=Limit_Min_Max(PID->i_out, -PID->i_max, PID->i_max);
  
  PID->output = PID->p_out + PID->i_out + PID->d_out;
  PID->output=Limit_Min_Max(PID->output, -PID->out_max, PID->out_max);
  return PID->output;
}

float PID_Calc_Speed(PID_struct_t *PID, float ref, float fdb)//PID运算函数（目标，实际）
{
  PID->ref = ref;
  PID->fdb = fdb;

  PID->err[1] = PID->err[0];
  PID->err[0] = PID->ref - PID->fdb;

  PID->p_out  = PID->kp * PID->err[0];
  PID->i_out += PID->ki * PID->err[0];
  PID->d_out  = PID->kd * (PID->err[0] - PID->err[1]);
  PID->i_out=Limit_Min_Max(PID->i_out, -PID->i_max, PID->i_max);
  
  PID->output = PID->p_out + PID->i_out + PID->d_out;
  PID->output=Limit_Min_Max(PID->output, -PID->out_max, PID->out_max);
  return PID->output;
}

/**
 * @brief 限制一个整数变量 value 在指定的最小值 min 和最大值 max 之间
 * @param value 输入值
 * @param min 最小值
 * @param max 最大值
 * @return 
 */
int Limit_Min_Max(int value,int min,int max)
{
	if(value<min)
		return min;
	else if(value>max)
		return max;
	else return value;
}
